#include <common.h>
#include <vmm.h>
#include <thread.h>

#define MK_FP(seg,off) ((unsigned int)(((unsigned int)(seg)<<16)|(unsigned short)(off)))
#define FP_SEG(fp) (((unsigned int)fp)>>16)
#define FP_OFF(fp) (((unsigned int)fp) & 0xFFFF)
#define FP_TO_LINEAR(seg,off) ((void *)((((unsigned short)(seg))<<4)+((unsigned short)off)))

unsigned int LinearToFp(void *ptr)
{
	unsigned seg, off;
	off = (unsigned int)ptr & 0xf;
	seg = ((unsigned int)ptr - ((unsigned int)ptr & 0xf));
	return MK_FP(seg,off);
}

#define VALID_FLAGS = 0xDFF;
//	0x87, 0xDB		MAGIC BREAKPOINT
unsigned char v86_code[]={
	0xcd, 0x10,  0xcd, 0x03
	};
void gpf_handler(registers *r);

void _v86_getbuf(registers *r)
{
	map(0x7000,pmm_alloc_page(), PAGE_PRESENT | PAGE_WRITE | PAGE_USER);
	r->eax = 0x7000;
}

void _v86_int(registers *r,unsigned int int_no, registers *v86r)
{
	register_int_handler(13, &gpf_handler);
	map(0x0000,0x0000, PAGE_PRESENT | PAGE_WRITE | PAGE_USER);		//Identify map 0x00000 - 0x10000 (IVT)
	unsigned int i = 0xA0;		//Identity map 0xA0000 - 0xFFFFF (Bios area)
	while(i <= 0xFF)
	{
		map(i*0x1000,i*0x1000, PAGE_PRESENT | PAGE_WRITE | PAGE_USER);
		i++;
	}
	v86_code[1] = int_no;
	memcopy(0x500,&v86_code[0], sizeof(unsigned char)*4);
	memcopy(&current_thread->v86_r,r,sizeof(registers));
	memcopy(r,v86r,sizeof(registers));

	r->ds = 0;
	r->cs = 0;
	r->ss = 0;	
	r->eflags = 0x20200;	//Fix this. Do I have to? No.
	r->useresp = 0xFF0;
	r->ebp = 0xFF0;
	r->eip = 0x500;
	current_thread->v86_if = 1;
}

void gpf_handler(registers *r)
{

	if(!(r->eflags & 0x20000))
	{
		printf("GPF!");
		printf("Unhandled interrupt no: %d Code:%x\n At:%x - %s\n",r->int_no,r->err_no, r->eip, panic_lookup(r->eip));

		printf(" ds:%x",r->ds);
		printf("\nedi:%x",r->edi);
		printf(" esi:%x",r->esi);
		printf(" ebp:%x",r->ebp);
		printf(" esp:%x",r->esp);
		printf(" ebx:%x",r->ebx);
		printf(" edx:%x",r->edx);
		printf(" ecx:%x",r->ecx);
		printf(" eax:%x",r->eax);
		printf("\nint_no:%x",r->int_no);
		printf(" err_code:%x",r->err_no);
		printf("\neip:%x",r->eip);
		printf(" cs:%x",r->cs);
		printf(" eflags:%x",r->eflags);
		printf(" useresp:%x",r->useresp);
		printf(" ss:%x",r->ss);
		panic("Fault!");
		for(;;);
	}
	unsigned char *ip;
	unsigned short *stack, *ivt;
	unsigned int *stack32;
	unsigned int is_o32, is_a32;
	
	ip = FP_TO_LINEAR(r->cs, r->eip);
	ivt = (unsigned short *)0x0;
	stack = (unsigned short *)FP_TO_LINEAR(r->ss,r->useresp);
	stack32 = (unsigned int *)stack;
	is_o32 = is_a32 = 0;

	while(1)
	{
	switch(ip[0])
	{
		case 0xcd:
			if(ip[1] == 0x3)
			{
				memcopy(current_thread->v86_r.ecx, r, sizeof(registers));
				memcopy(r,&current_thread->v86_r, sizeof(registers));
				return;
			}		
			stack -= 3;
			r->useresp = ((r->useresp & 0xFFFF) -6) & 0xFFFF;
			stack[0] = (unsigned short)(r->eip + 2);
			stack[1] = r->cs;
			stack[2] = (unsigned short)r->eflags ^ 0x200;
			r->cs = ivt[ip[1]*2+1];
			r->eip = ivt[ip[1]*2];
			return;
		case 0x66:
			printf("032");
			BREAK
			for(;;);
		case 0x67:
			printf("A32");
			BREAK
			for(;;);
		case 0x9c:
//			printf("pushf");
			if(is_o32)
			{
				printf("IS 032");
				BREAK
				for(;;);
			} else {
				r->useresp = ((r->useresp & 0xFFFF) - 2) & 0xFFFF;
				stack--;
				stack[0] = (unsigned short)r->eflags;
				
				if(current_thread->v86_if)
					stack[0] | 0x200;
				else
					stack[0] ^ 0x200;
			}
			r->eip = (unsigned short)(r->eip + 1);
			return;
		case 0x9d:
//			printf("popf");
			if(is_o32)
			{
				printf("POPF IS O32");
				BREAK
				for(;;);
			} else {
				r->eflags = 0x20200 | stack[0];
				current_thread->v86_if = (stack[0] & 0x200) != 0;
				r->useresp = ((r->useresp & 0xFFFF) + 2) & 0xFFFF;
			}
			r->eip = r->eip + 1;
			return;
		case 0xcf:
//			printf("iret");
			r->eip = stack[0];
			r->cs = stack[1];
			r->eflags = stack[2] | 0x20200;
			r->useresp = ((r->useresp & 0xffff) + 6) & 0xffff;
			return;
		case 0xfa:
//			printf("cli");
			current_thread->v86_if = 0;
			r->eip = r->eip + 1;
			return;
			for(;;);
		case 0xfb:
//			printf("sti");
			current_thread->v86_if = 1;
			r->eip = r->eip + 1;
			return;
			for(;;);
		case 0xee:
//			printf("outb(%x,%x)",r->edx&0xFFFF,r->eax&0xFF);	
			outb(r->edx & 0xffff, r->eax & 0xff);
			r->eip = r->eip + 1;
			return;
		case 0xef:
//			printf("outw(%x,%x)", r->edx&0xFFFF,r->eax);
			outw(r->edx & 0xFFFF, r->eax);
			r->eip = r->eip + 1;
			return;
		case 0xec:
//			printf("inb(%x)",r->edx & 0xFFFF);
			r->eax = inb(r->edx & 0xFFFF) & 0xFF;
			r->eip = r->eip + 1;
			return;
		case 0xed:
//			printf("inw(%x)",r->edx & 0xFFFF);
			r->eax = inw(r->edx & 0xFFFF) & 0xFFFF;
			r->eip = r->eip +1;
			return;
		default:
			printf("Something else: %x", ip[0]);
			BREAK
			for(;;);
	}
	}

}
